% 滑模观测器与超螺旋趋近律控制系统仿真
% 被控对象: G(s) = Kωₙ² / (s² + 2ζωₙs + ωₙ²)

clear; clc; close all;

%% 1. 系统参数定义
% 被控对象参数
omega_n = 1030.44;    % 谐振频率
zeta = 0.03;        % 阻尼比
K = 0.45157;             % 系统增益

% 计算传递函数系数
a0 = omega_n^2;                % 二阶项系数
a1 = 2 * zeta * omega_n;       % 一阶项系数
b0 = K * omega_n^2;            % 输入系数

% 观测器参数
w0 = omega_n * 5;  % 观测器带宽 (5-10倍omega_n)
l1 = 3 * w0;
l2 = 3 * w0^2;
l3 = w0^3;

% 滑模控制器参数
c1 = 100;             % 滑模面系数
k1=20;
L=0;
lambda1 = sqrt(k1+L)+0.1;         % 超螺旋趋近律系数1
lambda2 =  k1;       % 超螺旋趋近律系数2

% 仿真参数
t_start = 0;       % 仿真开始时间
t_end = 4;         % 仿真结束时间
t_step = 1e-6;     % 仿真步长

%% 2. 参考信号生成
t = t_start:t_step:t_end;
N = length(t);

% 参考信号: 初始为0，0.002s后变为1（阶跃信号）
% 参考信号: （正弦信号）
x_r = zeros(1, N);
for i = 1:N
    % if t(i) >= 0.002
    %     x_r(i) = 1;  % 阶跃幅值
    % end
    x_r(i)=sin(t(i)*pi);
end

% 参考信号的导数   
dx_r = zeros(1, N);
for i = 2:N
    dx_r(i) = (x_r(i) - x_r(i-1)) / t_step;
end

%% 3. 初始化状态变量
% 系统真实状态
x1 = zeros(1, N);  % 位置
x2 = zeros(1, N);  % 速度

% 观测器状态
x1_hat = zeros(1, N);  % 位置估计
x2_hat = zeros(1, N);  % 速度估计
x3_hat = zeros(1, N);  % 扰动估计

% 控制相关变量
s = zeros(1, N);       % 滑模面
s_int = zeros(1, N);   % 积分项
u = zeros(1, N);       % 控制输入

% 误差变量（用于相平面分析）
pos_error = zeros(1, N);  % 位置误差: x_r - x1
vel_error = zeros(1, N);  % 速度误差: dx_r - x2

%% 4. 仿真主循环
for k = 1:N-1
    % 当前参考信号
    xr = x_r(k);
    dxr = dx_r(k);
    
    % 计算误差（用于相平面）
    pos_error(k) = xr - x1(k);
    vel_error(k) = dxr - x2(k);
    
    % 计算观测误差
    ex1 = x1(k) - x1_hat(k);  % 位置误差
    
    % 观测器非线性项 (加入小epsilon避免数值问题)
    epsilon = 1e-8;
    L1 = l1 * (abs(ex1) + epsilon)^(2/3) * sign(ex1);
    L2 = l2 * (abs(ex1) + epsilon)^(1/3) * sign(ex1);
    L3 = l3 * sign(ex1);
    
    % 更新观测器状态
    x1_hat_dot = x2_hat(k) + L1;
    x2_hat_dot = -a0 * x1_hat(k) - a1 * x2_hat(k) + b0 * u(k) + L2 + x3_hat(k);
    x3_hat_dot = L3;
    
    x1_hat(k+1) = x1_hat(k) + x1_hat_dot * t_step;
    x2_hat(k+1) = x2_hat(k) + x2_hat_dot * t_step;
    x3_hat(k+1) = x3_hat(k) + x3_hat_dot * t_step;
    
    % 计算滑模面
    s(k) = c1 * (xr - x1(k)) + (dxr - x2_hat(k));
    
    % 计算超螺旋趋近律
    s_int_dot = lambda2 * sign(s(k));
    s_int(k+1) = s_int(k) + s_int_dot * t_step;
    ds = -lambda1 * (abs(s(k)) + epsilon)^(1/2) * sign(s(k)) - s_int(k);
    
    % 计算控制输入u
    u(k+1) = (1/b0) * (c1 * dxr - c1 * x2(k) + a0 * x1(k) + a1 * x2(k) ...
             - L2 - x3_hat(k) - ds);
    
    % 更新系统真实状态 (被控对象动力学)
    x1_dot = x2(k);
    x2_dot = -a0 * x1(k) - a1 * x2(k) + b0 * u(k);
    
    x1(k+1) = x1(k) + x1_dot * t_step;
    x2(k+1) = x2(k) + x2_dot * t_step;
end

% 补充最后一个时刻的误差值
pos_error(N) = x_r(N) - x1(N);
vel_error(N) = dx_r(N) - x2(N);

%% 5. 性能指标计算

% 时域性能指标计算（针对阶跃响应）
% 找到参考信号开始变化的索引（0.002s处）
step_index = find(t >= 0.002, 1);
t_step_response = t(step_index:end);
x1_step_response = x1(step_index:end);

% 稳态值（阶跃响应的最终值）
steady_state_value = x1(end);

% 最大超调量和峰值时间
[max_value, max_idx] = max(x1_step_response);
max_overshoot = ((max_value - steady_state_value) / steady_state_value) * 100;  % 百分比
max_overshoot1 = exp((-zeta*pi)/(sqrt(1-zeta^2)))
peak_time = t_step_response(max_idx);
peak_time1=(pi)/(omega_n*sqrt(1-zeta^2))

% 调整时间（5%误差带）
tolerance = 0.05 * steady_state_value;  % 5%误差带
% 找到最后一次超出误差带的索引
for i = length(x1_step_response):-1:1
    if abs(x1_step_response(i) - steady_state_value) > tolerance
        settling_time = t_step_response(i);
        break;
    end
    % 如果所有点都在误差带内，则取最后一个点的时间
    settling_time = t_step_response(1);
end

settling_time1=(3)/(zeta*omega_n)
% 频域性能指标计算
% 创建开环传递函数 G(s) = Kωₙ² / (s² + 2ζωₙs + ωₙ²)
num = [b0];
den = [1, a1, a0];
sys = tf(num, den);

% 计算相角裕度和幅值裕度
[Gm, Pm, Wcg, Wcp] = margin(sys);
% 转换为分贝
Gm_dB = 20 * log10(Gm);

%% 6. 结果可视化

% 位置跟踪曲线
figure('Name', '位置跟踪', 'Position', [100 100 800 500]);
plot(t, x_r, 'r--', 'LineWidth', 1.5);
hold on;
plot(t, x1, 'b', 'LineWidth', 1.5);
xlabel('时间 (s)');
ylabel('位置');
legend('参考位置', '实际位置');
title('位置跟踪性能');
grid on;

% 状态观测结果
figure('Name', '状态观测', 'Position', [100 200 800 500]);
subplot(2,1,1);
plot(t, x1, 'b', t, x1_hat, 'r--', 'LineWidth', 1.2);
xlabel('时间 (s)');
ylabel('位置');
legend('实际位置', '估计位置');
title('状态观测结果');
grid on;

subplot(2,1,2);
plot(t, x2, 'b', t, x2_hat, 'r--', 'LineWidth', 1.2);
xlabel('时间 (s)');
ylabel('速度');
legend('实际速度', '估计速度');
grid on;

% 控制信号与滑模面
figure('Name', '控制信号与滑模面', 'Position', [100 300 800 600]);
subplot(2,1,1);
plot(t, u, 'g', 'LineWidth', 1.2);
xlabel('时间 (s)');
ylabel('控制输入 u');
title('控制信号');
grid on;

subplot(2,1,2);
plot(t, s, 'm', 'LineWidth', 1.2);
xlabel('时间 (s)');
ylabel('滑模面 s');
title('滑模面变化');
grid on;

% 观测误差
figure('Name', '观测误差', 'Position', [100 400 800 500]);
plot(t, x1 - x1_hat, 'c', t, x2 - x2_hat, 'm--', 'LineWidth', 1.2);
xlabel('时间 (s)');
ylabel('误差');
legend('位置观测误差', '速度观测误差');
title('状态观测误差');
grid on;

% 误差相平面与滑模面轨迹
figure('Name', '误差相平面与滑模面', 'Position', [100 500 800 600]);
% 绘制系统状态轨迹
plot(pos_error, vel_error, 'b', 'LineWidth', 1.2);
hold on;

% 绘制滑模面 s = 0 (c1*pos_error + vel_error = 0)
pos_range = linspace(min(pos_error), max(pos_error), 100);
sliding_line = -c1 * pos_range;
plot(pos_range, sliding_line, 'r--', 'LineWidth', 1.5);

% 标记轨迹起点和终点
plot(pos_error(1), vel_error(1), 'go', 'MarkerSize', 8, 'LineWidth', 1.5);  % 起点
plot(pos_error(end), vel_error(end), 'ro', 'MarkerSize', 8, 'LineWidth', 1.5);  % 终点

% 添加时间点标记，显示轨迹方向
time_markers = 1:100:N;  % 每隔100个点标记一次
plot(pos_error(time_markers), vel_error(time_markers), 'ko', 'MarkerSize', 4);

xlabel('位置误差 (x_r - x)');
ylabel('速度误差 ($\dot{x}_r - \dot{x}$)','Interpreter','latex');
legend('系统状态轨迹', '滑模面 s=0', '起点', '终点', '时间标记');
title('误差相平面与滑模面轨迹');
grid on;
axis equal;  % 等比例显示，确保滑模面斜率正确

% 性能指标显示
figure('Name', '系统性能指标', 'Position', [100 600 600 400]);
text(0.1, 0.9, '系统性能指标', 'FontSize', 14, 'FontWeight', 'bold');
text(0.1, 0.75, ['最大超调量: ', num2str(max_overshoot, '%.4f'), '%'], 'FontSize', 12);
text(0.1, 0.65, ['峰值时间: ', num2str(peak_time, '%.4f'), ' s'], 'FontSize', 12);
text(0.1, 0.55, ['调整时间 (5%误差带): ', num2str(settling_time, '%.4f'), ' s'], 'FontSize', 12);
text(0.1, 0.40, ['相角裕度: ', num2str(Pm, '%.4f'), ' 度'], 'FontSize', 12);
text(0.1, 0.30, ['幅值裕度: ', num2str(Gm_dB, '%.4f'), ' dB'], 'FontSize', 12);
axis off;  % 关闭坐标轴

% 频域特性图
figure('Name', '系统频域特性', 'Position', [100 700 800 500]);
margin(sys);
title('系统Bode图与稳定裕度');

%% 7. 仿真结果分析与输出
max_pos_error = max(abs(x1 - x_r));
steady_pos_error = abs(x1(end) - x_r(end));

fprintf('位置跟踪性能指标:\n');
fprintf('  最大位置跟踪误差: %.4e\n', max_pos_error);
fprintf('  稳态位置跟踪误差: %.4e\n', steady_pos_error);
fprintf('\n时域性能指标:\n');
fprintf('  最大超调量: %.4f%%\n', max_overshoot);
fprintf('  峰值时间: %.4f s\n', peak_time);
fprintf('  调整时间 (5%%误差带): %.4f s\n', settling_time);
fprintf('\n频域性能指标:\n');
fprintf('  相角裕度: %.4f 度\n', Pm);
fprintf('  幅值裕度: %.4f dB\n', Gm_dB);
fprintf('  穿越频率: %.4f rad/s\n', Wcg);
fprintf('  截止频率: %.4f rad/s\n', Wcp);
fprintf('\n控制信号指标:\n');
fprintf('  最大控制输入: %.4f\n', max(abs(u)));

%% 8. 性能指标解释说明
disp(' ');
disp('性能指标解释说明:');
disp('1. 最大超调量: 系统输出超过稳态值的最大百分比，反映系统的相对稳定性');
disp('   数值越小，说明系统的相对稳定性越好，振荡越小');

disp('2. 峰值时间: 系统输出达到最大超调量的时间，反映系统的响应速度');
disp('   数值越小，说明系统对输入变化的响应越快');

disp('3. 调整时间: 系统输出进入并保持在稳态值5%误差范围内所需的时间');
disp('   数值越小，说明系统达到稳定状态的速度越快');

disp('4. 相角裕度: 系统开环相频特性达到-180度时，开环幅频特性与0dB线的距离');
disp('   通常相角裕度在30-60度之间较为理想，反映系统的相对稳定性');

disp('5. 幅值裕度: 系统开环幅频特性达到0dB时，相频特性与-180度的差值');
disp('   幅值裕度越大，系统的稳定性越好，通常大于6dB较为理想');
